// -*- coding: utf-8 -*-
#include "commander.h"
#include <math.h>
#include <locale>
#include <codecvt>

void createPoleSurroundTestMission(WaypointManager &manager)
{
    ROS_WARN("Creating Pole Surround Test Mission...");
    manager.reset();

    // 首先启用Offboard模式并解锁飞机
    manager.addOffboardEnable("offboard_enable_and_arm");
    
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addMovePoint(3.0, 2.0, 1.5, "takeoff_to_height");

    manager.addPoleSurround(3.0, 3.0, 1.0, 1.5, true, "performPoleSurrounding");
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addRotation(0.0, "scan_front");
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");

    manager.addSafeLanding("safe_landing");

    manager.finalize();
    ROS_WARN("Pole Surround Test Mission Created successfully!");
}

void createPatrolMissionWithPole(WaypointManager &manager)
{
    ROS_WARN("Creating Patrol Mission with Pole Surround...");
    manager.reset();
    
    // 首先启用Offboard模式并解锁飞机
    manager.addOffboardEnable("offboard_enable_and_arm");
    
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addMovePoint(3.0, 0.0, 1.5, "patrol_point_1");
    manager.addRotation(M_PI / 2, "scan_right");
    manager.addHover(3.0, "observe_1");
    manager.addMovePoint(3.0, 3.0, 1.5, "patrol_point_2");
    manager.addRotation(M_PI, "scan_back");
    manager.addHover(3.0, "observe_2");
    manager.addMovePoint(0.0, 2.0, 1.5, "patrol_point_3");
    manager.addRotation(-M_PI / 2, "scan_left");
    manager.addHover(3.0, "observe_3");
    manager.addPoleSurround(0, 3, 1.0, 1.5, true, "performPoleSurrounding");
    manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    manager.addRotation(0.0, "scan_front");
    manager.addHover(3.0, "observe_4");
    manager.addSafeLanding("safe_landing");
    manager.finalize();
    ROS_WARN("Patrol Mission with Pole Surround Created successfully!");
}

void createPatrolMissionWithTopic(WaypointManager &manager)
{
    ROS_WARN("Creating Patrol Mission with Topic Control...");
    manager.reset();
    
    // 首先启用Offboard模式并解锁飞机
    manager.addOffboardEnable("offboard_enable_and_arm");
    
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addMovePoint(3.0, 0.0, 1.5, "patrol_point_1");
    // manager.addTopicControl("/control/velocity_cmd", 10000000000, "vision_target_control"); //super-planner
    // manager.addTopicControl("/control/velocity_cmd", 10000000000, "vision_target_control");


    manager.addSuperControl(8, 6, 1.5, "/control/velocity_cmd", 1000000000, "patrol_point_2");
    manager.addMovePoint(8, 6, 1.5, "patrol_point_2");
    manager.addSuperControl(5, -8, 1.5, "/control/velocity_cmd", 1000000000, "patrol_point_2");

    // manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    // manager.addTopicControl("/vision_murmur", 15.0, "vision_target_control");
    // manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    // manager.addMovePoint(2.2, 2.2, 1.5, "patrol_point_5");
    // manager.addTopicControl("/vision_mur", 15.0, "vision_target_control");
    // manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    // manager.addRotation(0.0, "scan_front");
    // manager.addHover(3.0, "observe_4");
    manager.addSafeLanding("safe_landing");
    manager.finalize();
    ROS_WARN("Patrol Mission with Pole Surround Created successfully!");
}
